#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "sensor_msgs/point_cloud2_iterator.hpp"
#include <chrono>
#include <random>
#include <cmath>

using namespace std::chrono_literals;

class LidarNode : public rclcpp::Node
{
public:
    LidarNode() : Node("lidar_node"), frame_count_(0)
    {
        // 创建点云发布者，话题为/lidar/points，队列大小为10
        pointcloud_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("lidar/points", 10);
        
        // 设置定时器，每200ms发布一次点云（5Hz）
        timer_ = this->create_wall_timer(
            200ms, 
            std::bind(&LidarNode::publish_pointcloud, this)
        );
        
        RCLCPP_INFO(this->get_logger(), "Lidar node initialized");
    }

private:
    void publish_pointcloud()
    {
        // 创建点云消息
        sensor_msgs::msg::PointCloud2 msg;
        msg.header.stamp = this->get_clock()->now();
        msg.header.frame_id = "lidar_link";
        
        // 定义点云字段（x, y, z坐标）
        msg.height = 1;  // 无序点云
        msg.width = 1000;  // 点的数量
        
        // 设置点云数据格式
        sensor_msgs::PointCloud2Modifier modifier(msg);
        modifier.setPointCloud2FieldsByString(1, "xyz");
        
        // 准备随机数生成器
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_real_distribution<> angle_dist(0, 2 * M_PI);
        std::uniform_real_distribution<> distance_dist(1, 10);
        std::uniform_real_distribution<> height_dist(-0.5, 0.5);
        std::uniform_real_distribution<> obj_dist(-0.5, 0.5);
        std::uniform_real_distribution<> obj_height_dist(-0.3, 0.3);
        
        // 填充点云数据
        sensor_msgs::PointCloud2Iterator<float> out_x(msg, "x");
        sensor_msgs::PointCloud2Iterator<float> out_y(msg, "y");
        sensor_msgs::PointCloud2Iterator<float> out_z(msg, "z");
        
        // 生成环境点（900个点）
        for (size_t i = 0; i < 900; ++i, ++out_x, ++out_y, ++out_z)
        {
            float angle = angle_dist(gen);
            float distance = distance_dist(gen);
            
            *out_x = distance * cos(angle);
            *out_y = distance * sin(angle);
            *out_z = height_dist(gen);
        }
        
        // 生成移动物体的点云（100个点）
        float x_obj = (frame_count_ % 9) - 4;  // 在-4到4之间移动
        for (size_t i = 0; i < 100; ++i, ++out_x, ++out_y, ++out_z)
        {
            *out_x = x_obj + obj_dist(gen);
            *out_y = 5 + obj_dist(gen);  // 在前方5米左右
            *out_z = obj_height_dist(gen);  // 高度范围明确为-0.3到0.3米
        }
        
        // 发布点云消息
        pointcloud_publisher_->publish(msg);
        
        // 每5帧打印一次日志
        if (frame_count_ % 5 == 0) {
            RCLCPP_INFO(this->get_logger(), "Published %d pointclouds", frame_count_);
        }
        
        frame_count_++;
    }
    
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
    int frame_count_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<LidarNode>());
    rclcpp::shutdown();
    return 0;
}
    